/*=========================================================================
#     FileName: wl_queue.c
#     Desc:
#     LastChange:
#     History:
============================================================================= */
#include "FreeRTOS.h"
#include "cmsis_os.h"
#include "wl_queue.h"
#include "stdio.h"
#include "amLog.h"

#define QUEUE_COMM_DEPTH         40
#define QUEUE_CTRL_DEPTH         20
#define QUEUE_SUBG_DEPTH         10
#define QUEUE_BMS_DEPTH          10
#define QUEUE_TCU_DEPTH          20
#define QUEUE_FAC_DEPTH          4

static QueueHandle_t  pQueueHandle[WL_QUEUE_MAX_ID];

#ifdef QUEUE_USAGE_ENABLE
static uint32_t QueueMaxUsed[WL_QUEUE_MAX_ID];
#endif

const RRQueue_t  stQueueList[WL_QUEUE_MAX_ID] =
{
    {"QCOMM",       QUEUE_COMM_DEPTH},
    {"QCTRL",       QUEUE_CTRL_DEPTH},
    {"QSUBG",       QUEUE_SUBG_DEPTH},
    {"QBMS",        QUEUE_BMS_DEPTH},
    {"QFAC",        QUEUE_FAC_DEPTH},
    {"QTCU",        QUEUE_TCU_DEPTH},
};

static uint32_t g_queue_loss[WL_QUEUE_MAX_ID] = {0};

void wl_queue_init(void)
{
    int i;
    for (i = 0; i < WL_QUEUE_MAX_ID; i++)
    {
        pQueueHandle[i] = xQueueCreate(stQueueList[i].u32QueueLength, sizeof(Msg_st));

        if (NULL == pQueueHandle[i])
        {
            //DBGI("!!!Creat queue fail %s\n", stQueueList[i].ps8Name);
            printf("!!! creat queue %s fail !!!\r\n", stQueueList[i].ps8Name);
        }
        #ifdef QUEUE_USAGE_ENABLE
        else
        {
            QueueMaxUsed[i] = 0;
        }
        #endif
    }
}

int32_t wl_queue_is_full(uint32_t u32Qid, uint8_t s32InIsr)
{
    int32_t s32Ret = -WL_QUEUE_ERROR_PARAM;
    
    if (u32Qid < WL_QUEUE_MAX_ID)
    {
        if (1 == s32InIsr)
        {
            s32Ret = (uxQueueMessagesWaitingFromISR(pQueueHandle[u32Qid]) == stQueueList[u32Qid].u32QueueLength);            
        }
        else
        {
            s32Ret = (uxQueueMessagesWaiting(pQueueHandle[u32Qid]) == stQueueList[u32Qid].u32QueueLength);
        }
    }
    
    // if(1 == s32Ret)  printf("queue %s is FULL\n", stQueueList[u32Qid].ps8Name);
    if(1 == s32Ret)
        g_queue_loss[u32Qid]++;
    return s32Ret;
}

int32_t wl_queue_is_empty(uint32_t u32Qid)
{
    int32_t s32Ret = -WL_QUEUE_ERROR_PARAM;
    
    if (u32Qid < WL_QUEUE_MAX_ID)
    {
       s32Ret = xQueueIsQueueEmptyFromISR(pQueueHandle[u32Qid]);           
    }
    
    return s32Ret;
}

int32_t wl_queue_receive(uint32_t u32Qid, void* pvBuffer, uint32_t u32TicksToWait)
{
    int32_t s32Ret = -WL_QUEUE_ERROR_PARAM;
  
    if ((u32Qid < WL_QUEUE_MAX_ID) && (NULL != pvBuffer))
    {
        s32Ret = xQueueReceive(pQueueHandle[u32Qid], pvBuffer, u32TicksToWait);
        s32Ret = (pdTRUE == s32Ret) ? WL_QUEUE_ERROR_NULL : WL_QUEUE_ERROR_RCV_FAILED;
    }
    
    return s32Ret;
}

QueueHandle_t wl_queue_handle(uint32_t u32Qid)
{
    return (u32Qid < WL_QUEUE_MAX_ID) ? pQueueHandle[u32Qid] : NULL;
}

int32_t wl_queue_send(uint32_t u32Qid,  uint16_t u16MsgId, uint16_t u16SubMsgId, uint32_t u32Param, uint32_t u32TicksToWait)
{
    Msg_st  stMsg;
    int32_t s32Ret = WL_QUEUE_ERROR_PARAM;

    if (u32Qid < WL_QUEUE_MAX_ID)
    {
        stMsg.u16MsgId      = u16MsgId;
        stMsg.u16SubMsgId   = u16SubMsgId;
        stMsg.u32Param      = u32Param;
        
        s32Ret = xQueueSend(pQueueHandle[u32Qid], &stMsg, u32TicksToWait);
        if (s32Ret != pdTRUE)
        {
           g_queue_loss[u32Qid]++;
        }
        #ifdef QUEUE_USAGE_ENABLE
        uint32_t num = uxQueueMessagesWaiting(pQueueHandle[u32Qid]);
        if(num > QueueMaxUsed[u32Qid])
        {
            QueueMaxUsed[u32Qid] = num;
        }
        #endif
    }
    
    return s32Ret;
}

int32_t wl_queue_send_isr(uint32_t u32Qid,  uint16_t u16MsgId, uint16_t u16SubMsgId, uint32_t u32Param)
{
    Msg_st stMsg;
    portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
    int32_t s32Ret = WL_QUEUE_ERROR_NULL; 
    
    if (u32Qid < WL_QUEUE_MAX_ID)
    {        
        stMsg.u16MsgId      = u16MsgId;
        stMsg.u16SubMsgId   = u16SubMsgId;
        stMsg.u32Param      = u32Param;
        
        s32Ret = xQueueSendFromISR(pQueueHandle[u32Qid], &stMsg, &xHigherPriorityTaskWoken);
        if (s32Ret != pdTRUE)
        {
           g_queue_loss[u32Qid]++;
   		    s32Ret = -WL_QUEUE_ERROR_SEND_FAILED;
        }
        #ifdef QUEUE_USAGE_ENABLE
        uint32_t num = uxQueueMessagesWaitingFromISR(pQueueHandle[u32Qid]);
        if(num > QueueMaxUsed[u32Qid])
        {
            QueueMaxUsed[u32Qid] = num;
        }
        #endif
        portEND_SWITCHING_ISR( xHigherPriorityTaskWoken );
    }
    return s32Ret;
}

#ifdef QUEUE_USAGE_ENABLE
int32_t wl_queue_max_used(uint32_t u32Qid)
{
    int32_t s32Ret = -WL_QUEUE_ERROR_PARAM; 
    if (u32Qid < WL_QUEUE_MAX_ID)
    {
        s32Ret = QueueMaxUsed[u32Qid];
        QueueMaxUsed[u32Qid] = 0;
    }
    return s32Ret;
}
#endif

int32_t wl_queue_length(uint32_t u32Qid)
{
    int32_t s32Ret = -WL_QUEUE_ERROR_PARAM; 
    if (u32Qid < WL_QUEUE_MAX_ID)
    {
        s32Ret = stQueueList[u32Qid].u32QueueLength;
    }
    return s32Ret;
}

void  wl_queue_status_display(void)
{
    for(uint8_t i=0; i<WL_QUEUE_MAX_ID; i++)
    {
        LOG_TS(AM_LOG_LEVEL_INFO, "QUEUE", "%d [%d %d %d]\n", i, stQueueList[i].u32QueueLength, QueueMaxUsed[i], g_queue_loss[i]);
    }
}

